' 27. aug 2010
' saved a few ms by not comparing every obj in getIntersecting with a for(i=0 until len) for(j=i+1 until len) loop!
' saved 4-5 ms! by using TLists.contains instead of a manual for eachin search.

' Collision obj pool removed because might get extreme amouts of overlapping objs...
Type TGrid
	Field boxes:TList '<TBoundingBox>
	Field blockmap:TBlock[]
	Field world_xmin#, world_ymin#
	Field world_xmax#, world_ymax#
	Field world_xsiz#, world_ysiz#
	
	Field grid_sx:Int = 0
	Field grid_sy:Int = 0
	Field cell_w:Int = 0
	Field cell_h:Int = 0
	
	Field collisionPairs:TList = New TList 'to be returned
	Field intersections:TList = New TList 'to be returned
	Field collObjPool:TCollisionPair[] = Null
		
	'How should world coordinates be handled? Worldmin world max and objs outside are ignored?
	Function Create:TGrid(xmin#, ymin#, xmax#, ymax#, grid_sx:Int, grid_sy:Int)
		Local tmp:TGrid = New TGrid
		tmp.grid_sx = grid_sx
		tmp.grid_sy = grid_sy
		tmp.world_xmin = xmin; tmp.world_ymin = ymin; 
		tmp.world_xmax = xmax; tmp.world_ymax = ymax; 
		tmp.world_xsiz = xmax-xmin
		tmp.world_ysiz = ymax-ymin
		tmp.cell_w = tmp.world_xsiz / grid_sx
		tmp.cell_h = tmp.world_ysiz / grid_sy
		
		tmp.blockmap = New TBlock[grid_sx*grid_sy]
		tmp.boxes = New TList
		tmp._initPairPool() 'resizes when objs are added
		
		For Local i:Int=0 Until grid_sx
		For Local j:Int=0 Until grid_sy
		  	tmp.blockmap[i*grid_sy+j] = New TBlock
		Next
		Next
		
		Return tmp
	End Function
	
	
	Method insertBox(box:TBoundingBox)
		Assert box.getWidth() < world_xsiz/grid_sx, "obj too big for grid."+box.getWidth()+" > "+world_xsiz/grid_sx  +"must be smaller than a grid cell/block"
		Assert box.getHeight() < world_ysiz/grid_sy,  "obj too big for grid."+box.getHeight()+" > "+world_ysiz/grid_sy +"must be smaller than a grid cell/block"

		
		boxes.addLast(box)
		_resizePairPool( boxes.count()+10 )
	End Method
	
	Method removeBox(box:TBoundingBox)
		boxes.remove(box)
	End Method
	
	Method initMap()
		_clear()
		_fillMap() 'fill uses 70-80 % of tot. frame time!	
	End Method
	
	Method getIntersectionsWith:TList( queryObj:TBoundingBox ) 
		intersections.clear()
				
		addIntersectionsInBlock(queryObj,queryObj.tl)
		addIntersectionsInBlock(queryObj,queryObj.tr)
		addIntersectionsInBlock(queryObj,queryObj.bl)
		addIntersectionsInBlock(queryObj,queryObj.br)
		
		Return intersections
	End Method
	
	Method addIntersectionsInBlock(queryObj:TBoundingBox, block:TBlock)
		If block = Null Return
		Local vertList:TList = block.vertexList
		
		Local a_link:TLink = vertList.FirstLink()
		While a_link <> Null
			Local otherObj:TBoundingBox = TBoundingBox( a_link._value )
			If otherObj = queryObj
				'continue, get next link
			Else
				If boxesIntersect(queryObj,otherObj)	
					intersections.addLast(otherObj)
				EndIf
			EndIf
			a_link = a_link.NextLink()
		Wend
	End Method
	
	Method getIntersecting:TList()

		collisionPairs.clear()
		
		Rem
		'compare each obj with every other
		Local numObjsAdded:Int = 0
		For Local i:Int=0 Until grid_sx
		For Local j:Int=0 Until grid_sy
		  	For Local b1:TBoundingBox = EachIn (blockmap[i*grid_sy+j].vertexList)
			For Local b2:TBoundingBox = EachIn (blockmap[i*grid_sy+j].vertexList)
				If b1=b2 Continue
				
				If boxesIntersect(b1,b2)
					Assert numObjsAdded < (collObjPool.dimensions()[0]-1), "TGrid.getIntersecting: too many collision pairs"
					Local pair:TCollisionPair = collObjPool[numObjsAdded]
					pair.set(b1,b2)
					collisionPairs.addLast(pair)
					numObjsAdded:+1
				EndIf 
			Next
			Next
		Next
		Next
		End Rem
		
		'optimization, compare this with following. Also uses links directly instead of iterator. 
		
		
		
		Local a:TBoundingBox
		Local b:TBoundingBox
		Local block:TBlock
		Local vertList:TList

		Local numPairsFound:Int = 0
		Local i:Int
		For i=0 Until Len( blockmap )
				block = blockmap[i]
				vertList = block.vertexList
				
				Local a_link:TLink = vertList.FirstLink()
				While a_link <> Null
					a = TBoundingBox( a_link._value )
					Local b_link:TLink = a_link.NextLink()
					
					While b_link <> Null
						b = TBoundingBox( b_link._value )
						'assert a<>b, "Same obj inserted twice into block!"
						
						If boxesIntersect(a,b)	
								Local isUnique:Int = True 'until proven non-uniq
								Local newPair:TCollisionPair = collObjPool[numPairsFound]
								newPair.set(a,b)
								
								'is pair unique?
								For Local oldPair:TCollisionPair = EachIn collisionPairs
									If newPair.equals(oldPair)
										isUnique = False
										Exit
									EndIf
								Next
								
								If isUnique
									collisionPairs.addLast(newPair)
									numPairsFound:+1
									If(numPairsFound > (collObjPool.dimensions()[0]-1)) _resizePairPool(numPairsFound + 10)
								EndIf
						EndIf
						b_link = b_link.NextLink()
					 Wend
					a_link = a_link.NextLink()
				Wend
		Next
		
		Return collisionPairs
	End Method
	
	Method _initPairPool()
		collObjPool = New TCollisionPair[100]
		For Local i:Int = 0 Until collObjPool.dimensions()[0]
			collObjPool[i] = New TCollisionPair
		Next
	End Method
		
	Method _resizePairPool(newSize:Int)
		If collObjPool.dimensions()[0] < newSize
			Local oldSiz:Int = collObjPool.dimensions()[0]-1
			collObjPool = collObjPool [..newSize] 'resize using blitzmax slices, is this better than making a new[]?
			For Local i:Int = oldSiz Until collObjPool.dimensions()[0]
				collObjPool[i] = New TCollisionPair
			Next
		EndIf
	End Method
	
	Method _clear()
		
		For Local i:Int = 0 Until grid_sx
		For Local j:Int = 0 Until grid_sy
			blockmap[i*grid_sy+j].num_vertices = 0
			blockmap[i*grid_sy+j].vertexList.clear()
		Next
		Next
		
	End Method 
	
	'fillmap combined with setblock take 60-70% of the collision testing time.
	'using a quadtree may be very much faster on this subproblem
	Method _fillMap()
		Local xscale# = 1.0/(world_xsiz/grid_sx)
		Local yscale# = 1.0/(world_ysiz/grid_sy)
		
	  For Local b:TBoundingBox = EachIn boxes
		'do not insert boxes that have an edge outside of the world grid
		If b.xmin < world_xmin Continue
		If b.ymin < world_ymin Continue
		If b.xmax > world_xmax Continue
		If b.ymax > world_ymax Continue		
		
		Local xTL# = Floor(b.xmin*xscale)
		Local yTL# = Floor(b.ymin*yscale)
		
		Local xTR# = Floor(b.xmax*xscale)
		Local yTR# = Floor(b.ymin*yscale)
		
		Local xBL# = Floor(b.xmin*xscale)
		Local yBL# = Floor(b.ymax*yscale)
		
		Local xBR# = Floor(b.xmax*xscale)
		Local yBR# = Floor(b.ymax*yscale)
		
		b.tl = _setBlock(xTL, yTL, b) 'minmin
		b.tr = _setBlock(xTR, yTR, b) 'maxmin
		b.bl = _setBlock(xBL, yBL, b) 'minmax
		b.br = _setBlock(xBR, yBR, b) 'maxmax
	  Next
	End Method 
	
	'assign a boundingbox to a given block in the blockmap[xp,yp]
	Method _setBlock:TBlock(xp:Int, yp:Int, insertbox:TBoundingBox )
		'fetch block at coordinate
		Local block:TBlock = blockmap[xp*grid_sy+yp]

		If block.vertexList.contains(insertbox)
			Return Null
		Else
			block.num_vertices :+ 1
	    		block.vertexList.addLast(insertBox)
			Return block
		EndIf
	End Method 
	
	Method drawGrid()
		SetColor(255,255,255)
		For Local i:Int=0 Until grid_sx
		For Local j:Int=0 Until grid_sy
			DrawRectOpen(i*cell_w, j*cell_h, (i+1)*cell_w, (j+1)*cell_h)
		Next
		Next
		
	End Method 
	
	Method drawConnections()
		SetColor(255,255,255)
		For Local i:Int=0 Until grid_sx
		For Local j:Int=0 Until grid_sy
			
			For Local b:TBoundingBox = EachIn blockmap[i*grid_sy+j].vertexList
				Local blockCenterx# = i*cell_w+cell_w*.5
				Local blockCentery# = j*cell_h+cell_h*.5
				DrawLine( blockCenterx,blockCentery,..
							  b.xmin+b.getWidth()*.5, b.ymin+b.getHeight()*.5 )
			Next
			
		Next
		Next
	End Method
	
End Type

Type TBlock
	Field num_vertices:Int = 0
	Field vertexList:TList = New TList
End Type

'can use it as an OBB, rotations are fine :)
Type TBoundingBox
	Field tl:TBlock = Null
	Field tr:TBlock = Null
	Field bl:TBlock = Null
	Field br:TBlock = Null

	Field xmin#, ymin#
	Field xmax#, ymax#
	
	Method setBlocks(tl:TBlock,tr:TBlock,bl:TBlock,br:TBlock)
		Self.tl = tl
		Self.tr = tr
		Self.bl = bl
		Self.br = br
	End Method
	
	Method setBounds(xmin#,ymin#,xmax#,ymax#)
		Self.xmin=xmin; Self.ymin=ymin
		Self.xmax=xmax; Self.ymax=ymax
	End Method
	
	Method getWidth:Float()
		Return xmax-xmin
	End Method
	
	Method getHeight:Float()
		Return ymax-ymin
	End Method
End Type

Type TCollisionPair
	Field a:TBoundingBox
	Field b:TBoundingBox
	Field collided:Int = False
	
	Method equals:Int(other:TCollisionPair)
		If(Self.a = other.a And Self.b = other.b) Return True
		If(Self.a = other.b And Self.b = other.a) Return True
		Return False
	End Method
	
	Method set(a:TBoundingBox, b:TBoundingBox)
		Self.a = a 
		Self.b = b
	End Method
End Type


Function drawrectopen(x0#,y0#,x1#,y1#)
	DrawLine(x0,y0,x1,y0)
	DrawLine(x1,y0,x1,y1)
	DrawLine(x1,y1,x0,y1)
	DrawLine(x0,y1,x0,y0)
End Function

Function boxesIntersect:Int(a:TBoundingBox , b: TBoundingBox )
  Return Not (( a.xmin > (b.xmax) ) Or ( (a.xmax) < (b.xmin) ) Or..
           ( (a.ymin) > (b.ymax) ) Or ( (a.ymax) < (b.ymin) ) )
End Function
